#ifndef SLAM_ROS_SLAM_ROS_SENSOR_DATA_CACHE_HPP
#define SLAM_ROS_SLAM_ROS_SENSOR_DATA_CACHE_HPP

#include <list>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include "geometry_msgs/msg/point_stamped.hpp"
#include "agv_msgs/msg/odometry_lite.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "slam_ros_msgs/msg/qr_code.hpp"

namespace slam_ros {

class SensorDataCache {
 public:
  SensorDataCache(
    const std::string & sensor_data_log_path,
    const float max_time_duration);
  ~SensorDataCache();

  void AddImuData(const sensor_msgs::msg::Imu::SharedPtr msg);
  void AddLaserData(const sensor_msgs::msg::LaserScan::SharedPtr msg);
  void AddOdomData(const agv_msgs::msg::OdometryLite::SharedPtr msg);
  void AddRobotPose(const geometry_msgs::msg::PointStamped::SharedPtr msg);
  void AddQRCode(const slam_ros_msgs::msg::QRCode::SharedPtr msg);

  float GetBufferMaxLengthInSec();
  void SwapSensorData();
  void LogSensorData();

 private:
  std::string sensor_data_log_path_;
  const float max_time_duration_;
  
  // ROS 2 uses shared pointers for messages
  std::unique_ptr<std::list<sensor_msgs::msg::Imu::SharedPtr>> imu_data_buffer_;
  std::unique_ptr<std::list<sensor_msgs::msg::Imu::SharedPtr>> imu_data_buffer_bak_;
  std::unique_ptr<std::list<agv_msgs::msg::OdometryLite::SharedPtr>> odom_data_buffer_;
  std::unique_ptr<std::list<agv_msgs::msg::OdometryLite::SharedPtr>> odom_data_buffer_bak_;
  std::unique_ptr<std::list<sensor_msgs::msg::LaserScan::SharedPtr>> laser_data_buffer_;
  std::unique_ptr<std::list<sensor_msgs::msg::LaserScan::SharedPtr>> laser_data_buffer_bak_;
  std::unique_ptr<std::list<geometry_msgs::msg::PointStamped::SharedPtr>> robot_pose_buffer_;
  std::unique_ptr<std::list<geometry_msgs::msg::PointStamped::SharedPtr>> robot_pose_buffer_bak_;
  std::unique_ptr<std::list<slam_ros_msgs::msg::QRCode::SharedPtr>> qrcode_buffer_;
  std::unique_ptr<std::list<slam_ros_msgs::msg::QRCode::SharedPtr>> qrcode_buffer_bak_;
};

} // namespace slam_ros

#endif